#include "abaqus_inp_parser.h"
#include "rotate.h"
#include <iostream>

// Instance method implementations
std::vector<float> Instance::getNodeCoord(int nodeID) {
    std::vector<float> coord = ref_part->getNodeCoord(nodeID);
    
    // Apply translation
    for (int i = 0; i < 3; i++) {
        coord[i] += translate[i];
    }
    
    // Apply rotation if exists
    if (!rotation.empty()) {
        Matrix<float> point_matrix(1, 3);
        for (int i = 0; i < 3; i++) {
            point_matrix(0, i) = coord[i];
        }
        
        Matrix<float> rotated = Rotate3D::rotate_3d_points(
            point_matrix, rotation[0], rotation[1], rotation[2][0]);
            
        for (int i = 0; i < 3; i++) {
            coord[i] = rotated(0, i);
        }
    }
    
    return coord;
}

std::pair<std::string, std::vector<int>> Instance::getElemInfo(int elemID) {
    return ref_part->getElemInfo(elemID);
}

// Part method implementations
void Part::elemSummary() {
    int sum = 0;
    for (const auto& pair : elem_types) {
        elem_type_offsets[pair.first] = sum;
        sum += pair.second.rows;
    }
}

int Part::getElemSeqIndex(int elem_id) {
    auto it = elem_info.find(elem_id);
    if (it == elem_info.end()) return -1;
    
    const auto& type_index = it->second;
    auto offset_it = elem_type_offsets.find(type_index.first);
    if (offset_it == elem_type_offsets.end()) return -1;
    
    return offset_it->second + type_index.second;
}

int Part::getElemTotalNum() const {
    int total = 0;
    for (const auto& pair : elem_types) {
        total += pair.second.rows;
    }
    return total;
}

std::vector<float> Part::getNodeCoord(int nodeID) {
    auto it = node_id_to_index.find(nodeID);
    if (it == node_id_to_index.end()) {
        return std::vector<float>(3, 0.0f);
    }
    
    int index = it->second;
    std::vector<float> coord(3);
    for (int i = 0; i < 3; i++) {
        coord[i] = node_array(index, i);
    }
    return coord;
}

std::pair<std::string, std::vector<int>> Part::getElemInfo(int elemID) {
    auto elem_it = elem_info.find(elemID);
    if (elem_it == elem_info.end()) {
        return std::make_pair("", std::vector<int>());
    }
    
    const auto& type_index = elem_it->second;
    auto type_it = elem_types.find(type_index.first);
    if (type_it == elem_types.end()) {
        return std::make_pair("", std::vector<int>());
    }
    
    const Matrix<int>& elem_array = type_it->second;
    int index = type_index.second;
    
    std::vector<int> nodes(elem_array.cols);
    for (int i = 0; i < elem_array.cols; i++) {
        nodes[i] = elem_array(index, i);
    }
    
    return std::make_pair(type_index.first, nodes);
}

// Assembly method implementations
void Assembly::addInstance(std::shared_ptr<Instance> instance) {
    instances[instance->getName()] = instance;
}

std::shared_ptr<Instance> Assembly::getInstance(const std::string& name) const {
    auto it = instances.find(name);
    return (it != instances.end()) ? it->second : nullptr;
}

std::vector<std::string> Assembly::getInstanceList() const {
    std::vector<std::string> names;
    for (const auto& pair : instances) {
        names.push_back(pair.first);
    }
    return names;
}

void Assembly::addPart(const std::string& name, std::shared_ptr<Part> part) {
    parts[name] = part;
}

std::shared_ptr<Part> Assembly::getPart(const std::string& name) const {
    auto it = parts.find(name);
    return (it != parts.end()) ? it->second : nullptr;
}